﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ARCam3D.Tuple;

namespace ARCam3D.Imaging.Processing
{
    public class CornersDetector
    {
        private int anglesRangesCount = 50;
        private int cornersCount = 4;
        private double localMaximumThresholdFactor = 0;

        public Point2I[] DetectCorners(List<Point2I> points, VectorImage vectorImage)
        {
            List<int> anglesRangesPointsCounts = new List<int>(new int[anglesRangesCount]);

            foreach (Point2I point in points)
            {
                double angle = new Vector2D(vectorImage.GetVectorX(point.x, point.y), vectorImage.GetVectorY(point.x, point.y)).PolarAngle;
                int angleRangeIndex = (int)(anglesRangesCount * angle / (2 * Math.PI)) % anglesRangesCount;
                anglesRangesPointsCounts[angleRangeIndex]++;
            }

            int localMaximumThreshold = 0;//(int)(points.Count * localMaximumThresholdFactor);
            List<Tuple<int, double>> anglesRangesPointsCountsLocalMaximums = new List<Tuple<int, double>>();

            for (int i = 0; i < anglesRangesPointsCounts.Count; ++i)
            {
                if (anglesRangesPointsCounts[i] - anglesRangesPointsCounts[Utils.GetModulo(i - 1, anglesRangesCount)] > localMaximumThreshold &&
                    anglesRangesPointsCounts[i] - anglesRangesPointsCounts[Utils.GetModulo(i + 1, anglesRangesCount)] > localMaximumThreshold)
                {
                    anglesRangesPointsCountsLocalMaximums.Add(new Tuple<int, double>(
                        anglesRangesPointsCounts[i], 
                        i * 2 * Math.PI / anglesRangesCount
                    ));
                }
            }

            //should be at least <cornersCount> local maximums
            if (anglesRangesPointsCountsLocalMaximums.Count < cornersCount)
            {
                return null;
            }

            //sorting local maximums by points count from high to low
            anglesRangesPointsCountsLocalMaximums.Sort(delegate(Tuple<int, double> a, Tuple<int, double> b) { 
                return b.Item1.CompareTo(a.Item1); 
            });

            List<Point2I>[] pointGroups = new List<Point2I>[cornersCount];
            List<double> angles = new List<double>(cornersCount);

            for (int i = 0; i < cornersCount; ++i)
            {
                pointGroups[i] = new List<Point2I>();
                angles.Add(anglesRangesPointsCountsLocalMaximums[i].Item2);
            }

            //sorting angles from low to high
            angles.Sort(delegate(double a, double b) { return a.CompareTo(b); });

            foreach (Point2I point in points)
            {
                double angle = new Vector2D(vectorImage.GetVectorX(point.x, point.y), vectorImage.GetVectorY(point.x, point.y)).PolarAngle;

                double minAngleDifference = 2 * Math.PI;
                int pointGroupIndex = 0;
                //double angleDifferenceThresold = Math.PI / 70;

                for (int i = 0; i < cornersCount; ++i)
                {
                    //shifting angle to 2PI and -2PI, so that angles near 0 and 2PI will have small difference
                    for (int k = -1; k <= 1; ++k)
                    {
                        double shiftedAngle = angle + k * 2 * Math.PI;
                        double angleDifference = Math.Sqrt((angles[i] - shiftedAngle) * (angles[i] - shiftedAngle));

                        if (angleDifference < minAngleDifference)
                        {
                            //pointGroups[i].Add(point);
                            minAngleDifference = angleDifference;
                            pointGroupIndex = i;
                        }
                    }
                }
                pointGroups[pointGroupIndex].Add(point);
            }

            Point2I[] corners = new Point2I[4];

            for (int i = 0; i < 4; ++i)
            {
                if (angles[i] >= 0 && angles[i] < Math.PI / 2)
                {
                    corners[i] = getLeftBottomPoint(pointGroups[i]);
                }
                else if (angles[i] >= Math.PI / 2 && angles[i] < 2 * Math.PI / 2)
                {
                    corners[i] = getTopLeftPoint(pointGroups[i]);
                }
                else if (angles[i] >= Math.PI && angles[i] <  3 * Math.PI / 2)
                {
                    corners[i] = getRightTopPoint(pointGroups[i]);
                }
                else if (angles[i] >= 3 * Math.PI / 2 && angles[i] < 2 * Math.PI)
                {
                    corners[i] = getBottomRightPoint(pointGroups[i]);
                }
                else
                {
                    return null;
                }

                if (corners[i] == null)
                {
                    return null;
                }
            }

            return corners;
        }

        private Point2I getBottomRightPoint(List<Point2I> points)
        {
            if (points.Count <= 0) return null;

            Point2I result = points[0];
            for (int i = 0; i < points.Count; ++i)
            {
                if (points[i].y > result.y)
                {
                    result = points[i];
                }
                else if (points[i].y == result.y)
                {
                    if (points[i].x > result.x)
                    {
                        result = points[i];
                    }
                }
            }
            return result;
        }

        private Point2I getRightTopPoint(List<Point2I> points)
        {
            if (points.Count <= 0) return null;

            Point2I result = points[0];
            for (int i = 0; i < points.Count; ++i)
            {
                if (points[i].x > result.x)
                {
                    result = points[i];
                }
                else if (points[i].x == result.x)
                {
                    if (points[i].y < result.y)
                    {
                        result = points[i];
                    }
                }
            }
            return result;
        }

        private Point2I getTopLeftPoint(List<Point2I> points)
        {
            if (points.Count <= 0) return null;

            Point2I result = points[0];
            for (int i = 0; i < points.Count; ++i)
            {
                if (points[i].y < result.y)
                {
                    result = points[i];
                }
                else if (points[i].y == result.y)
                {
                    if (points[i].x < result.x)
                    {
                        result = points[i];
                    }
                }
            }
            return result;
        }

        private Point2I getLeftBottomPoint(List<Point2I> points)
        {
            if (points.Count <= 0) return null;

            Point2I result = points[0];
            for (int i = 0; i < points.Count; ++i)
            {
                if (points[i].x < result.x)
                {
                    result = points[i];
                }
                else if (points[i].x == result.x)
                {
                    if (points[i].y > result.y)
                    {
                        result = points[i];
                    }
                }
            }
            return result;
        }
    }
}
